/*
 * RobotDrive2012.hpp
 *
 *  Created on: Feb 11, 2012
 *      Author: trwulff
 */

/*
 * Description:
 */

#include "Constants.hpp"
#include "WPILib.h"
// #defines for different drive modes
#define FAULT_TIME 1
#define KPERCENTVBUS
#ifdef KPERCENTVBUS
#define MODE "KPERCENTVBUS"
#endif
//#define KSPEED
#ifdef KSPEED
#define MODE "KSPEED"
#endif
//#define KCURRENT
#ifdef KCURRENT
#define MODE "KCURRENT"
#endif
#ifdef KPERCENTVBUS
#define DEFAULTP -0.25	
#define DEFAULTI -.01	
#define DEFAULTD 0	
#define CONTROLMODE CANJaguar::kPercentVbus
#define MAX_RPM 1
#define VOLTAGE_RAMP 120 
#endif 
#ifdef KSPEED
#define DEFAULTP -0.25	//Speed mode
#define DEFAULTI -.01		//Speed Mode
#define DEFAULTD 0		//Speed Mode
#define CONTROLMODE CANJaguar::kSpeed
#define MAX_RPM 1000
#endif
#ifdef KCURRENT
#define CONTROLMODE CANJaguar::kCurrent
#define DEFAULTP 1000	//Current mode
#define DEFAULTI .1		//Current Mode
#define DEFAULTD 1		//Current Mode
#define MAX_RPM 40
#endif
#define ENCODER_LPR 255 // 250 lpr in quadrature mode (4x)

#ifndef ROBOTDRIVE2012_HPP_
#define ROBOTDRIVE2012_HPP_

#define CALIBRATION_TIME 2

class RobotDrive2012
{
public:
	/*
	 * Default constructor
	 */
	RobotDrive2012(UINT8 LeftMotorChannel=2,UINT8 RightMotorChannel=3,float P=DEFAULTP,float I=DEFAULTI,float D=DEFAULTD,UINT16 EncoderLPR=ENCODER_LPR);

	/*
	 * Default destructor
	 */
	~RobotDrive2012();
	void SetPID(double P,double I,double D);
	void ArcadeDrive(float RtLft, float FwdRev);
	void SetEncoderLPR(UINT16 LPR);
	void SetSafetyEnabled (bool safetyEnabled);
	void SetExpiration (float timeout);
	void Drive(float outputMagnitude, float curve);
	void SetNeutral(CANJaguar::NeutralMode mode=CANJaguar::kNeutralMode_Brake);
	void PivotDrive (float leftDriveValue, float rightDriveValue);
	float Max(float V1,float V2);
	float Limit(float);
	void Printf();
	void ChangeToPosition();
	void SetPosition(float Position);
	bool Calibrate();
	void Status();
	CANJaguar Jag1, Jag2;
	

protected:

private:
	float PP,II,DD;
	UINT16 LLPR;
	int iCount;
	Timer CalibrationTimer;
	bool CalFlg;
};
#endif
